/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <vector>
#include "HeightMapDisplay.hpp"
#include "readtex.h"
#include <assert.h>

using namespace std;

static int hm_dimX = 0;
static int hm_dimZ = 0;
static int hm_graphObject_ListID = -1;

static GLuint	texture[1];
static char *texname = "groundtexture.rgb";

/**
* Call this from your display func
*/
void hm_draw(float x, float y, float z){
  if(hm_graphObject_ListID > 0){
    glPushMatrix();
	  glTranslatef(x, y, z);
    glCallList(hm_graphObject_ListID);
    glPopMatrix();
  }  
}

static void texUnbind()
{
  glDisable(GL_TEXTURE_2D);
}

static void texBind(){
	glEnable(GL_TEXTURE_2D);						// Enable Texture Mapping
	glBindTexture(GL_TEXTURE_2D, texture[0]);
  glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
}

static void texLoad(){
  // Set as UNPACK_ALIGNMENT  
  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
  assert(glGetError() == GL_NO_ERROR);

  glGenTextures(1, &texture[0]);			// Create One Textures
  assert(glGetError() == GL_NO_ERROR);

  glBindTexture(GL_TEXTURE_2D, texture[0]);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
  assert(glGetError() == GL_NO_ERROR);
  int r = LoadRGBMipmaps(texname, GL_RGB);
  assert (r == GL_TRUE);
}

//int hm_generateHeightMapDisplayList(TGroundModelMap *map)

int hm_generateHeightMapDisplayList(TGroundModelMap *map)
{
    int dimX=map->getNX();
    int dimZ=map->getNY(); 

    float xRatio= map->getResolution(); 
    float yRatio=1.0f; 
    float zRatio=map->getResolution();
    int step=1;
    int c_x_begin=0, 
      c_z_begin=0, 
      c_x_end = map->getNX(), 
      c_z_end = map->getNY();
    bool drawNormals = false;
    bool doTexture = true;

    int graphObject_ListID=0;

    TGraphPointVector normalVectorList;
    TGraphVector n;
    float **heightMap = NULL;
    TGraphVector **normalMap= NULL;

    hm_dimX = dimX;
    hm_dimZ = dimZ;

    fprintf(stderr,"-----------------hm_generateHeightMapDisplayList----------------- \n");
    fprintf(stderr,"hm_generateHeightMapDisplayList:: Map size %d x %d = %d\n",dimX,dimZ,dimX*dimZ);
    fprintf(stderr,"hm_generateHeightMapDisplayList:: Reserving memory....");
    // Reserve memory for the height map
   // Create list of pointers for X
    heightMap = new float * [dimX];
    // Create a array for Z:s
    for(int i=0; i < dimX; i++) {
        heightMap[i] = new float [dimZ];
    }
    
    /// Reserve memory for the Normal map
    normalMap = new TGraphVector* [dimX];
    for(int i=0; i < dimX; i++) {
        normalMap[i] = new TGraphVector [dimZ];
    }
    fprintf(stderr,"hm_generateHeightMapDisplayList:: Done\n");
    fprintf(stderr,"hm_generateHeightMapDisplayList:: Copying data and generating Normals....");
    //// Make stupid copy
    for(int i=0;i<map->getNX();i++){
        for(int j=0;j<map->getNY();j++){
            TBasePixel pix = map->getPixel(i,j);
        
            heightMap[i][j]= (float) (pix.v) / 100.0;
	}
    }
    for(int i=0;i<map->getNX();i++){
        for(int j=0;j<map->getNY();j++){
            normalMap[i][j]=hm_constructNormalForHeightMapPoint(heightMap,i, j,xRatio, 1.0f,zRatio);
	}
    }
    fprintf(stderr,"Done\n");
      
  // If local list is uninitialized:
  if (heightMap == NULL) {
    fprintf(stderr,"WARNING: Tried to generate a display list from uninitializes map!");
    return -1;
  }

  // Check for trivial errors
  if (c_x_end < c_x_begin || c_z_end < c_z_begin) return -1;

  int xb=c_x_begin, 
      zb=c_z_begin, 
      xe=c_x_end, 
      ze=c_z_end;

  // Assign the local start/end pointers
  if (c_x_begin < 0) xb = 0;
  if (c_x_end < 0) xe = dimX-1; 
  if (c_z_begin < 0) zb = 0;
  if (c_z_end < 0) ze = dimZ-1;

  if (c_x_end > dimX-1) xe = dimX-1;
  if (c_z_end > dimZ-1) ze = dimZ-1;
  
  graphObject_ListID = glGenLists(1); 
  if (graphObject_ListID == 0){
    fprintf(stderr,"Error! List id is Zero Error='%s'\n",gluErrorString(glGetError()));  
    return -1;  
  }
  //GLfloat amb[4] = {1.0, 1.0, 1.0, 1.00};
  //GLfloat diff[4] = {0.8, 0.8, 0.8, 1.00};
	GLfloat amb[4] = {0.5, 0.5, 0.5, 0.5};
	GLfloat diff[4] = {0.5, 0.5, 0.5, 0.5};
	GLfloat glfModelSpecular[4];

	// This causes the polygon's ambient & diffuse colors to be derived from the glColor() value.
	
	
	/* Set the specular shading color. */
	
	//glColor3f(r_diffuseColor, g_diffuseColor, b_diffuseColor);

	
	
  if (doTexture) {
    texLoad();
  }

  glNewList(graphObject_ListID, GL_COMPILE);
  glPushAttrib(GL_COLOR_BUFFER_BIT | GL_ENABLE_BIT);
 
  glEnable(GL_LIGHTING);
  glEnable(GL_CULL_FACE); // Cull the "frontside" of this object
  glCullFace(GL_FRONT);
  
  glEnable(GL_LIGHT1); // FIXME?
	
/*
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, amb);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diff);
	*/
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 30.0);
	
	/*
	glfModelSpecular[0] = 1;
	glfModelSpecular[1] = 1;
	glfModelSpecular[2] = 1;
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, glfModelSpecular);
	*/
  if (doTexture) {
    texBind();
  }

  fprintf(stderr,"hm_generateHeightMapDisplayList:: Starting the GL STUFF\n");
//  float xPart = 1.0, zPart = 1.0;
//  float xPartStep = 1.00/(float)(xe-xb), zPartStep = 1.00/(float)(ze-zb);
//  float xPartStep = 2*(1.00/(float)(xe-xb)), zPartStep = 2*(1.00/(float)(ze-zb));
//  float xPartStep = 2.0*(1.00/(float)(ze-zb)), zPartStep = 2.0*(1.00/(float)(ze-zb));

	normalVectorList.clear();
  for(int ix=xb; ix <= xe-step; ix += step) {
    glBegin(GL_TRIANGLE_STRIP);
  
//    zPart = 1.0;
    for(int iz=zb; iz <= ze; iz += step) {
      if (doTexture) {
        glTexCoord2f(1.0, (iz%2) ? 1.0 : 0.0);
/*        float tz = zPart-zPartStep;
        if (tz < 0.00) tz = 0.00;
        glTexCoord2f(xPart-xPartStep, (iz%2) ? zPart : tz);*/
//        glTexCoord2f(xPart, (iz%2) ? zPart : zPart-zPartStep);
      }
 
      if (normalMap != NULL) { 
        glNormal3f( normalMap[ix][iz].x, 
                    normalMap[ix][iz].y,
                    normalMap[ix][iz].z);
      }
      n.x =ix*xRatio;
      n.y =heightMap[ix][iz] * yRatio;
      n.z =iz*zRatio;

      glVertex3f(n.x , n.y, n.z);
      if (drawNormals) {
        normalVectorList.push_back(n);
				TGraphVector v = normalMap[ix][iz];
				v.x = v.x * 0.1;
				v.y = v.y * 0.1;
				v.z = v.z * 0.1;
        normalVectorList.push_back(n+v);
      }

      int ixt = ix+step;
       
      if (doTexture) {
        glTexCoord2f(0.0, (iz%2) ? 1.0 : 0.0);
/*        float tz = zPart-zPartStep;
        if (tz < 0.00) tz = 0.00;
        glTexCoord2f(xPart-xPartStep, (iz%2) ? zPart : tz);*/
      }
  
      if (normalMap != NULL) { 
        glNormal3f( normalMap[ixt][iz].x, 
                    normalMap[ixt][iz].y,
                    normalMap[ixt][iz].z );
      }

      //// FIXMEE:: NO SCALING IF FLOATS
      n.x =ixt*xRatio;
      n.y =heightMap[ixt][iz] * yRatio;
      n.z =iz*zRatio;

      glVertex3f(n.x , n.y, n.z);
      if (drawNormals) {
        normalVectorList.push_back(n);
        normalVectorList.push_back(n+normalMap[ixt][iz]);
      }
    }
    glEnd(); 
  }
  
  if (doTexture) {
    texUnbind();
  }

  if (drawNormals) {    
    glDisable(GL_LIGHTING);
    glEnable(GL_DEPTH_TEST);
		glBegin(GL_LINES);
		glColor3f(1.0, 0.0, 0.0);
    for(TGraphPointVectorIterator i = normalVectorList.begin();
        i != normalVectorList.end();
        i++) {
			glVertex3f(i->x, i->y, i->z);
    }
    glEnd();
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_LIGHTING);

	}

  glPopAttrib();
  glEndList();
    

		fprintf(stderr,"hm_generateHeightMapDisplayList:: GL STUFF DONE\n");
		fprintf(stderr,"hm_generateHeightMapDisplayList:: Freeing reserved memory\n");
  //FREE MEMORY
  if (normalMap != NULL) {
    for(int i=0; i < dimX; i++) {
      delete[] normalMap[i];
    }
    delete[] normalMap;
    normalMap = NULL;
  }

  if (heightMap != NULL) {
    for(int i=0; i < dimX; i++) {
      delete[] heightMap[i];
    }
    delete[] heightMap;
    heightMap = NULL;
  }

    fprintf(stderr,"hm_generateHeightMapDisplayList:: done\n");

  // Store to global 
  hm_graphObject_ListID = graphObject_ListID;

  return graphObject_ListID;
}


//******************************************************************************
//******************************************************************************

#define LIMITX(vv) (MIN(MAX(vv, 0), hm_dimX-1)) 
#define LIMITZ(vv) (MIN(MAX(vv, 0), hm_dimZ-1))
#define PT_X(px,pz) ((LIMITX(px))*xR)
#define PT_Y(px,pz) (heightMap[(LIMITX(px))][(LIMITZ(pz))]*yR)
#define PT_Z(px,pz) ((LIMITZ(pz))*zR)
#define SET_PT(pnp, qx, qz) pnp.x = PT_X(qx,qz); pnp.y = PT_Y(qx,qz); pnp.z = PT_Z(qx,qz); 
TGraphVector hm_constructNormalForHeightMapPoint(float **heightMap,int x, int z,
                                                             float xR, float yR,
                                                             float zR)
{
 TGraphPoint p = { x*xR , heightMap[x][z] * yR, z*zR }; // 'Le point' ;)
 TGraphPoint np; // New point
 TGraphPointVector pv; // The neighbours point array


 // Add neighbour point coords to array 
 SET_PT(np, x-1, z);  pv.push_back(np);
 SET_PT(np, x-1, z+1);  pv.push_back(np);
 SET_PT(np, x, z+1);  pv.push_back(np);
 SET_PT(np, x+1, z+1);  pv.push_back(np);
 SET_PT(np, x+1, z);  pv.push_back(np);
 SET_PT(np, x+1, z-1);  pv.push_back(np);
 SET_PT(np, x, z-1);  pv.push_back(np);
 SET_PT(np, x-1, z-1);  pv.push_back(np);
 
 return hm_calculateVertexNormal(p, pv); // Return the calculated normal
}

float hm_calculatePointDistance( float *p1, float *p2 )
{
  float  
    a = (p2[X_AX] - p1[X_AX]),
    b = (p2[Y_AX] - p1[Y_AX]),
    c = (p2[Z_AX] - p2[Z_AX]);
#ifdef OS_WINDOWS
  return (float)sqrt((a*a)+(b*b)+(c*c));
#else
  return sqrtf((a*a)+(b*b)+(c*c)); // Faster
#endif //OS_WINDOWS  
}                                                        
//******************************************************************************

TGraphVector hm_calculateVertexNormal(const TGraphPoint &vertex, 
                                                const TGraphPointVector &near_)
{
 TGraphVector n;
 TGraphTriangle tmpTriangle;
 
 n.x = 0.00;
 n.y = 0.00;
 n.z = 0.00;

 // Calculate sum of face normals
 tmpTriangle.a = vertex; // copy first triangle here
 for (vector<TGraphVector>::const_iterator i = near_.begin();
      i != near_.end();
      i++) {
   tmpTriangle.b = *i;
   i++;
   tmpTriangle.c = *i;
   n = n + hm_calculateFaceNormal(tmpTriangle);
 }
 
 // Return normalized sum
 return hm_normalizeVector(n);
}
//******************************************************************************

TGraphVector hm_calculateFaceNormal(const TGraphTriangle &t)
{
 TGraphVector v1 = t.b - t.a;
 TGraphVector v2 = t.c - t.a;
 TGraphVector ncp = hm_crossProductVector(v1, v2);
 TGraphVector vn = hm_normalizeVector(ncp);
 return vn;
}
//******************************************************************************

TGraphVector hm_normalizeVector(const TGraphVector &v)
{
 TGraphVector n;
 float l;

 // Calculate length, divide all components by length -> unit vector!
 l = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
 n.x = v.x / l;
 n.y = v.y / l;
 n.z = v.z / l;
 
 return n;
}
//******************************************************************************

TGraphVector hm_crossProductVector(const TGraphVector &v1, 
                                             const TGraphVector &v2)
{
 // Calculate cross product between the two 3 compo vectors
 TGraphVector n;
 n.x = v1.y * v2.z - v1.z * v2.y;
 n.y = v1.z * v2.x - v1.x * v2.z;
 n.z = v1.x * v2.y - v1.y * v2.x;
 return n;
}
//**********************************************************************//

//******************************************************************************/
bool operator == (const TGraphVector &a, const TGraphVector &b)
{
   //dPrint("%.5f %.5f %.5f == %.5f %.5f %.5f",a.x,a.y,a.z,b.x,b.y,b.z);
   if (fabs(a.x-b.x) > 0.0001) return false;
   if (fabs(a.y-b.y) > 0.0001) return false;
   if (fabs(a.z-b.z) > 0.0001) return false;
   return true;
}
//******************************************************************************

TGraphVector operator + (const TGraphVector &a, const TGraphVector &b)
{
   TGraphVector n;
   n.x = a.x + b.x;
   n.y = a.y + b.y;
   n.z = a.z + b.z;
   return n;
}
//******************************************************************************

TGraphVector operator - (const TGraphVector &a, const TGraphVector &b)
{
   TGraphVector n;
   n.x = a.x - b.x;
   n.y = a.y - b.y;
   n.z = a.z - b.z;
   return n;
}
//******************************************************************************
//******************************************************************************

